#!/usr/bin/env python
# -*- coding: UTF-8 -*-

from os import stat
import numpy as np
import math
import matplotlib.pyplot as plt
import time

k = 0.1 # 前视距离系数
Lfc = 0.5 # 前视距离
Kp = 1.0 # 速度P控制器系数
dt = 0.1 # 时间间隔，单位：s
L = 0.1 # 车辆轴距，单位：m
vbar = 1 # 预计车速, 单位：m/s

class VehicleState:
    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, w=0.0, vr=0.0, vl=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.w = w
        self.vr = vr
        self.vl = vl

class MyPath:
    def __init__(self,x,y):
        n = int((x**2+y**2)**0.5 * 10) # 10cm 取模得到路径插值点数
        print(n)
        if abs(x) < 0.1:
            #直线差值
            self.cx = np.arange(0, x, n)
            self.cy = np.arange(0, y, n)
        else:
            #贝塞尔
            P0,P1,P2 = np.array([[0,0],[x,0],[x,y]])# 在此处修改坐标
            P = lambda t:(1-t)**2*P0 + 2*t*(1-t)*P1 + t**2*P2
            points = np.array([P(t) for t in np.linspace(0,1,n)])

            self.cx, self.cy = points[:,0], points[:,1]


        #self.cx = np.arange(0, 50, 1)
        #self.cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in self.cx]

def update(state, vr, vl):
    # state.x = state.x + state.v * math.cos(state.yaw) * dt
    # state.y = state.y + state.v * math.sin(state.yaw) * dt
    # state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
    # state.v = state.v + a * dt
    dtheta = (vr - vl)*dt/L
    R = L*(vr+vl)/(2*(vr-vl))
    dl = 2*R*np.sin(vbar*dt/(2*R))
    #state.x = state.x + dl * np.cos(dtheta/2)
    #state.y = state.y + dl * np.sin(dtheta/2)
    state.v = vbar
    state.x = state.x + state.v * math.cos(state.yaw) * dt
    state.y = state.y + state.v * math.sin(state.yaw) * dt
    state.yaw = state.yaw - dtheta 
    state.w = (vr-vl)/L
    state.vr = vr
    state.vl = vl

    # public the state to the motor 

    print(dl)
    print(state.yaw*180/np.pi)

    return state

def PControl(target, current):
    a = Kp * (target - current)

    return a

def calc_target_index(state, cx, cy):
    # 搜索最临近的路点
    dx = [state.x - icx for icx in cx]
    dy = [state.y - icy for icy in cy]
    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
    ind = d.index(min(d)) # 找出最小的d所在的位置
    J = 0.0

    Lf = k * state.v + Lfc

    while Lf > J and (ind + 1) < len(cx):
        dx = cx[ind + 1] - cx[ind]
        dy = cy[ind + 1] - cy[ind]
        J += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1
        #print(J)

    return ind

def pure_pursuit_control(state, cx, cy, pind):
    ind = calc_target_index(state, cx, cy) #搜寻最近的点 ind

    if pind >= ind:
        ind = pind

    if ind < len(cx): #若目标点没有超过范围，去具体坐标赋予 tx，ty用作目标
        tx = cx[ind]
        ty = cy[ind]
    else: #若超过了，把最后一个点赋给目标
        tx = cx[-1]
        ty = cy[-1]
        ind = len(cx) - 1
        
    alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw #计算当前点到目标点的方向角差
    
    if state.v < 0: # back
        alpha = math.pi - alpha

    Lf = k * state.v + Lfc #前视距离

    # 前视距离的选取与速度有关，也与单位时间距离有关
    # delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) #计算转向角
    ld = ((ty-state.y)**2+(tx-state.x)**2)**0.5
    vr = vbar - L*vbar*np.sin(alpha)/(2*ld)
    vl = 2*vbar-vr
    
    return vr, vl, ind

def ReadSpeed():
    vr = 1
    vl = 0

    return vr, vl

def main(tx,ty):
    # 生成目标路点
    # cx = np.arange(0, 50, 1)
    # cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
    path = MyPath(tx,ty)
    cx = path.cx
    cy = path.cy

    #target_speed = 10.0 / 3.6 # [m/s]

    T = 100.0 # 最大模拟时间

    init_vr, init_vl = ReadSpeed()
    # 设置车辆的初始状态
    state = VehicleState(x=-0.0, y=-0.0, yaw=0.0, v=2.0, w=0.0, vr=init_vr, vl=init_vl)

    lastIndex = len(cx) - 1
    mytime = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    w = [state.w]
    t = [0.0]
    target_ind = calc_target_index(state, cx, cy)

    while T >= mytime and lastIndex >= target_ind:
        #ai = PControl(target_speed, state.v)
        #di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
        tvr, tvl, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
        state = update(state, tvr, tvl)

        mytime = mytime + dt

        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        w.append(state.w)
        t.append(mytime)
        print(lastIndex)
        print(target_ind)

        plt.cla()
        plt.plot(cx, cy, ".r", label="course")
        plt.plot(x, y, "-b", label="trajectory")
        plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
        plt.axis("equal")
        plt.grid(True)
        plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
        plt.pause(0.001)

        dis = (state.x-path.cx[lastIndex])**2-(state.y-path.cy[lastIndex])
        if dis < Lfc:
            break
        #plt.show
        #time.sleep(0.001)
    

if __name__ == '__main__':
    main(5,6)